#include <iostream>

#include <cxcore.h>
#include <cv.h>

#include "median.hh"


double median(const CvMat *arr)
{

    CvSize sz = cvGetSize(arr);
    double med;
    CvMat *mat = cvCloneMat(arr);

    if(CV_MAT_DEPTH(arr->type) == CV_64F){
	double *data;
	cvGetRawData(mat, (unsigned char**)&data);
	
	std::sort(data, data+(sz.width*sz.height));
	
	if((sz.width*sz.height)%2 == 0){
	    int n = sz.width*sz.height/2;	
	    med = (data[n-1]+data[n])/2;		
	}
	else{
	    int n = (int)floor(sz.width*sz.height/2);
	    med = data[n];
	}    
    }
    else if(CV_MAT_DEPTH(arr->type) == CV_32F){
	float *data;
	cvGetRawData(mat, (unsigned char**)&data);
	
	std::sort(data, data+(sz.width*sz.height));
	
	if((sz.width*sz.height)%2 == 0){
	    int n = sz.width*sz.height/2;	
	    med = (data[n-1]+data[n])/2;		
	}
	else{
	    int n = (int)floor(sz.width*sz.height/2);
	    med = data[n];
	}    
    }
    cvReleaseMat(&mat);
    
    return med;

}


double median(const IplImage *img)
{

    CvMat mat;

    cvGetMat(img, &mat);
    
    return median(&mat);

//     IplImage *order;
//     double *data;
//     CvSize sz = cvGetSize(img);
//     double med;

//     order = cvCloneImage(img);    
    
//     cvGetRawData(order, (unsigned char**)&data);
    
//     std::sort(data, data+(sz.width*sz.height));
    
//     if((sz.width*sz.height)%2 == 0){
// 	int n = sz.width*sz.height/2;	
// 	med = (data[n-1]+data[n])/2;		
//     }
//     else{
// 	int n = (int)floor(sz.width*sz.height/2);
// 	med = data[n];
//     }    

//     cvReleaseImage(&order);

//     return med;
}
